#include "uavmath.h"

const matrix_t MATRIX_IDENTITY = {
	{ FIXED_RESOLUTION }, { 0LL },              { 0LL },
	{ 0LL },              { FIXED_RESOLUTION }, { 0LL },
	{ 0LL },              { 0LL },              { FIXED_RESOLUTION }
};

void matrix_mul(const matrix_t *A, const matrix_t *B, matrix_t *out)
{
	out->m11 = fixed_mul(A->m11, B->m11);
	out->m11.val += fixed_mul(A->m12, B->m21).val + fixed_mul(A->m13, B->m31).val;
	out->m12 = fixed_mul(A->m11, B->m12);
	out->m12.val += fixed_mul(A->m12, B->m22).val + fixed_mul(A->m13, B->m32).val;
	out->m13 = fixed_mul(A->m11, B->m13);
	out->m13.val += fixed_mul(A->m12, B->m23).val + fixed_mul(A->m13, B->m33).val;
	
	out->m21 = fixed_mul(A->m21, B->m11);
	out->m21.val += fixed_mul(A->m22, B->m21).val + fixed_mul(A->m23, B->m31).val;
	out->m22 = fixed_mul(A->m21, B->m12);
	out->m22.val += fixed_mul(A->m22, B->m22).val + fixed_mul(A->m23, B->m32).val;
	out->m23 = fixed_mul(A->m21, B->m13);
	out->m23.val += fixed_mul(A->m22, B->m23).val + fixed_mul(A->m23, B->m33).val;
	
	out->m31 = fixed_mul(A->m31, B->m11);
	out->m31.val += fixed_mul(A->m32, B->m21).val + fixed_mul(A->m33, B->m31).val;
	out->m32 = fixed_mul(A->m31, B->m12);
	out->m32.val += fixed_mul(A->m32, B->m22).val + fixed_mul(A->m33, B->m32).val;
	out->m33 = fixed_mul(A->m31, B->m13);
	out->m33.val += fixed_mul(A->m32, B->m23).val + fixed_mul(A->m33, B->m33).val;
}

void matrix_add(const matrix_t *A, const matrix_t *B, matrix_t *out)
{
	out->m11.val = A->m11.val + B->m11.val;
	out->m12.val = A->m12.val + B->m12.val;
	out->m13.val = A->m13.val + B->m13.val;
	
	out->m21.val = A->m21.val + B->m21.val;
	out->m22.val = A->m22.val + B->m22.val;
	out->m23.val = A->m23.val + B->m23.val;
	
	out->m31.val = A->m31.val + B->m31.val;
	out->m32.val = A->m32.val + B->m32.val;
	out->m33.val = A->m33.val + B->m33.val;
}

void matrix_sub(const matrix_t *A, const matrix_t *B, matrix_t *out)
{
	out->m11.val = A->m11.val - B->m11.val;
	out->m12.val = A->m12.val - B->m12.val;
	out->m13.val = A->m13.val - B->m13.val;
	
	out->m21.val = A->m21.val - B->m21.val;
	out->m22.val = A->m22.val - B->m22.val;
	out->m23.val = A->m23.val - B->m23.val;
	
	out->m31.val = A->m31.val - B->m31.val;
	out->m32.val = A->m32.val - B->m32.val;
	out->m33.val = A->m33.val - B->m33.val;
}

fixed_t matrix_det(const matrix_t *A)
{
	fixed_t det = {
		fixed_mul(A->m11, fixed_sub(fixed_mul(A->m22, A->m33), fixed_mul(A->m23, A->m32))).val
		+ fixed_mul(A->m12, fixed_sub(fixed_mul(A->m23, A->m31), fixed_mul(A->m21, A->m33))).val
		+ fixed_mul(A->m13, fixed_sub(fixed_mul(A->m21, A->m32), fixed_mul(A->m22, A->m31))).val
	};
	return det;
}

void matrix_inverse(const matrix_t *A, matrix_t *out)
{
	out->m11.val = fixed_mul(A->m22, A->m33).val - fixed_mul(A->m23, A->m32).val;
	out->m12.val = fixed_mul(A->m13, A->m32).val - fixed_mul(A->m12, A->m33).val;
	out->m13.val = fixed_mul(A->m12, A->m23).val - fixed_mul(A->m13, A->m22).val;
	
	out->m21.val = fixed_mul(A->m23, A->m31).val - fixed_mul(A->m21, A->m33).val;
	out->m22.val = fixed_mul(A->m11, A->m33).val - fixed_mul(A->m13, A->m31).val;
	out->m23.val = fixed_mul(A->m13, A->m21).val - fixed_mul(A->m11, A->m23).val;
	
	out->m31.val = fixed_mul(A->m21, A->m32).val - fixed_mul(A->m22, A->m31).val;
	out->m32.val = fixed_mul(A->m12, A->m31).val - fixed_mul(A->m11, A->m32).val;
	out->m33.val = fixed_mul(A->m11, A->m22).val - fixed_mul(A->m12, A->m21).val;
	
	fixed_t det = fixed_mul(A->m11, out->m11);
	det.val += fixed_mul(A->m12, out->m21).val + fixed_mul(A->m13, out->m31).val;
	fixed_t inv_det = fixed_div(FIXED_ONE, det);
	
	out->m11 = fixed_mul(out->m11, inv_det);
	out->m12 = fixed_mul(out->m12, inv_det);
	out->m13 = fixed_mul(out->m13, inv_det);
	
	out->m21 = fixed_mul(out->m21, inv_det);
	out->m22 = fixed_mul(out->m22, inv_det);
	out->m23 = fixed_mul(out->m23, inv_det);
	
	out->m31 = fixed_mul(out->m31, inv_det);
	out->m32 = fixed_mul(out->m32, inv_det);
	out->m33 = fixed_mul(out->m33, inv_det);
}

void matrix_transpose(const matrix_t *A, matrix_t *out)
{
	out->m11 = A->m11;
	out->m12 = A->m21;
	out->m13 = A->m31;
	
	out->m21 = A->m12;
	out->m22 = A->m22;
	out->m23 = A->m32;
	
	out->m31 = A->m13;
	out->m32 = A->m23;
	out->m33 = A->m33;
}
